//motor A connected between A01 and A02
//motor B connected between B01 and B02
//设置标志使能 
int STBY  = 40; //standby
int STBY2 = 41; //standby2
//设置了四个电机的PWM引脚和电机输入口
//Motor A
int PWMA = 2; //Speed control 
int AIN1 = 30; //Direction
int AIN2 = 31; //Direction
//Motor B
int PWMB = 3; //Speed control
int BIN1 = 32; //Direction
int BIN2 = 33; //Direction
//Motor C
int PWMC = 4; //Speed control
int CIN1 = 34; //Direction
int CIN2 = 35; //Direction
//Motor D
int PWMD = 10; //Speed control
int DIN1 = 48; //Direction
int DIN2 = 49; //Direction
int i;
String aa;
void setup(){
  pinMode(STBY , OUTPUT);
  pinMode(STBY2, OUTPUT);
	
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
 
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  pinMode(PWMC, OUTPUT);
  pinMode(CIN1, OUTPUT);
  pinMode(CIN2, OUTPUT);

  pinMode(PWMD, OUTPUT);
  pinMode(DIN1, OUTPUT);
  pinMode(DIN2, OUTPUT);

  Serial.begin(9600);//可换成蓝牙控制
  Serial2.begin(9600);
}
/* 先完成前进后退侧移
 *  已有函数void move(int motor, int speed, int direction)
 *  定义了三个参数，    motor,speed,direction
 *  分别是  选择电机，pwm大小（1-255），顺逆时针旋转（AO1 AO2 的高低电平定位）
 *  可 停止操作    stop()
 * 	麦克纳姆轮运作.xlsx
	分类了麦轮车运动的10中分类
	
	先完成四类直线路径
	半速前进
	move(a,128,0)顺时针
	move(b,128,0)顺时针
	
	未改进地方。pwm控制的大小不可控，即是，车速不可控。
	
	需要额外添加 遥感 来追加，模拟信号写入。
	或者 遥感+单片机 做成遥控 ，信息用 zigbee 协调机+zigbee 终端机  互传。
	  pos1 = map(analogRead(A5), 0, 1280, 245, 10);   //遥感参数读取，并映射
	  pos2 = map(analogRead(A6), 0, 1280, 245, 10);	  //遥感参数读取，并映射
	  OUT = '[' + String(pos1) + ',' + String(pos2) + ']';//数据整合
	读取遥感的xy轴。映射对应pwm波参数，供给给电机控制速度。 
	
	pwm参数确定以后。。。
	怎么给电机，需不需要分开给？
	pwm控制后   变速运动，会造成什么后果，有点
	可以加油门，，，，
	打满才会全速？确认这点。
	
	假设已经完美控制小车，下一步是？
	额外追加串口控制摄像头。openmv端追加串口函数，串口读取。
	设置三种模式。开关图传，手动，自动扫描，追踪模式。
		传输规则  
		从终端机--协调器（主控板一体）
		启动时，自组网，自动配网。完成通信连接。通信连接以后，可解锁操作。
		初始化后，解锁，手动驾驶，手动摄像头 模式，
		
		
 */
void loop(){
	if(Serial2.available())
	{
			 String aa = "";
	
			while(Serial2.available()>0)
			{
				
				aa += char(Serial2.read());
				delay(20);
			}
			if (aa.length() > 0)//如果comdata有数据
			{
			  Serial.println(aa);//打印comdata数据
			}
			scan();
			delay(50);
	}

	if(Serial.available())//接受串口信号
	{
		i=Serial.read();//信号赋值
		//2020.3.1
		//已完成，完成了9+2运动
		switch(i)//判断参数，执行命令
		{
		//完成无人车移动姿势，所有运动为匀速移动。
		
		case'1'://前进
			Serial.println("go");
			move(1,128,0);//左前轮a逆时针
			move(2,128,1);//右前轮b顺时针
			move(3,128,0);  
			move(4,128,1);
			break;
		case'2'://后退
			Serial.println("back");
			move(1,128,1);
			move(2,128,0);
			move(3,128,1);
			move(4,128,0);
			break;
		case'3'://左移
				//需要更大的力气
			Serial.println("左侧移");
			move(1,140,1);
			move(2,140,1);
			move(3,140,1);  
			move(4,140,1);
			break;
		case'4'://右移
			Serial.println("顺时针");
			move(1,140,0);
			move(2,140,0);
			move(3,140,0);  
			move(4,140,0);
			break;
		case'a':// a 以后  按 b 不能转动
				//现象  往前会向前，
				//		往后会向后
				//左前拐弯完成 不受影响
				//发现没break
			Serial.println("");
			move(1,0,1);
			move(4,0,1);
			move(2,128,1);
			move(3,128,0);
			move(1,0,1);
			move(4,0,1);
			break;
		case'b'://测试时候按下1以后会转需要修改v1
				//v2 只会后转
		//左后拐弯  不受影响
			Serial.println("");
			move(1,0,0);
			move(4,0,0);
			move(2,128,0);
			move(3,128,1);
			move(1,0,0);
			move(4,0,0);
			
			break;
		case'c'://是右前转
			Serial.println("右前转");
			move(1,128,0);
			
//			digitalWrite(AIN1, LOW);
//			digitalWrite(AIN2, HIGH);
//			analogWrite(PWMA, 128);
			move(4,128,1);
//			digitalWrite(DIN1, HIGH);
//			digitalWrite(DIN2, LOW);
//			analogWrite(PWMD, 128);  
			move(2,0,0);
			move(3,0,0);
			break;
		case'd'://是右后转
			Serial.println("右后转");
			move(1,128,1);
//			digitalWrite(AIN1, HIGH);
//			digitalWrite(AIN2, LOW);
//			analogWrite(PWMA, 128);
			move(4,128,0);
//			digitalWrite(DIN1, LOW);
//			digitalWrite(DIN2, HIGH);
//			analogWrite(PWMD, 128);
			move(2,0,0);
			move(3,0,0);
			break;
		case'e'://原地左转
			
			Serial.println("move");
			move(1,128,1);
			move(2,128,1);
			move(3,128,0);
			move(4,128,0);
			break;
		case'f'://原地左转右，v0__发现没break
			Serial.println("move");
			move(3,128,1);
			move(4,128,1);
			move(1,128,0);
			move(2,128,0);
			break;		
		case'0'://停止
			Serial.println("access 3");
			Serial.println("stop");
			stop();
		
		/*	双轮移动测试成功了
			可以开始下一步
			
			四轮移动，包括
			前进后退，左移，右移。//直线运动，
			左前转弯，左后转弯。右前转，右后转//曲线运动。
			以上测试是匀速测试
		
		*/
		}
	}
  //写入新循环，包括。
  //1。蓝牙串口接受，
  //2.运动分析。
  //  麦轮小车
  //3.
  //原始循环
  /*
  move(1, 255, 1); //motor 1, full speed, left
  move(2, 255, 1); //motor 2, full speed, left
 
  delay(1000); //go for 1 second
  stop(); //stop
  delay(250); //hold for 250ms until move again
 
  move(1, 128, 0); //motor 1, half speed, right
  move(2, 128, 0); //motor 2, half speed, right
 
  delay(1000);
  stop();
  delay(250);
  */
  
}
 
 //该函数，可定义   轮胎id， pwm速度 ，顺逆方向
void move(int motor, int speed, int direction){
//Move specific motor at speed and direction
//motor: 0 for B 1 for A
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise
 
  digitalWrite(STBY , HIGH); //disable standby
  digitalWrite(STBY2, HIGH);
	
  boolean inPin1 = LOW;
  boolean inPin2 = HIGH;
//  boolean inPin1 = LOW;
//  boolean inPin2 = HIGH;
  if(direction == 1){
    inPin1 = HIGH;
    inPin2 = LOW;
  }
 
  if(motor == 1){
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
  }else if(motor==2){
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
  }else if(motor==3){
	digitalWrite(CIN1, inPin1);
    digitalWrite(CIN2, inPin2);
    analogWrite(PWMC, speed);  
  }else if(motor==4){
	digitalWrite(DIN1, inPin1);
    digitalWrite(DIN2, inPin2);
    analogWrite(PWMD, speed);  
  }
}
 
void stop(){
//enable standby  
  digitalWrite(STBY , LOW); 
  digitalWrite(STBY2, LOW);
}
void scan()
{
		i=aa.toInt();

		switch(i)//判断参数，执行命令
		{
		case'1'://前进
			Serial.println("go");
			move(1,128,0);//左前轮a逆时针
			move(2,128,1);//右前轮b顺时针
			move(3,128,0);  
			move(4,128,1);
			break;
		case'2'://后退
			Serial.println("back");
			move(1,128,1);
			move(2,128,0);
			move(3,128,1);
			move(4,128,0);
			break;
		case'3'://左移
				//需要更大的力气
			Serial.println("左侧移");
			move(1,140,1);
			move(2,140,1);
			move(3,140,1);  
			move(4,140,1);
			break;
		case'4'://右移
			Serial.println("顺时针");
			move(1,140,0);
			move(2,140,0);
			move(3,140,0);  
			move(4,140,0);
			break;
		case'a':// a 以后  按 b 不能转动
				//现象  往前会向前，
				//		往后会向后
				//左前拐弯完成 不受影响
				//发现没break
			Serial.println("");
			move(1,0,1);
			move(4,0,1);
			move(2,128,1);
			move(3,128,0);
			move(1,0,1);
			move(4,0,1);
			break;
		case'b'://测试时候按下1以后会转需要修改v1
				//v2 只会后转
		//左后拐弯  不受影响
			Serial.println("");
			move(1,0,0);
			move(4,0,0);
			move(2,128,0);
			move(3,128,1);
			move(1,0,0);
			move(4,0,0);
			
			break;
		case'c'://是右前转
			Serial.println("右前转");
			move(1,128,0);
			
//			digitalWrite(AIN1, LOW);
//			digitalWrite(AIN2, HIGH);
//			analogWrite(PWMA, 128);
			move(4,128,1);
//			digitalWrite(DIN1, HIGH);
//			digitalWrite(DIN2, LOW);
//			analogWrite(PWMD, 128);  
			move(2,0,0);
			move(3,0,0);
			break;
		case'd'://是右后转
			Serial.println("右后转");
			move(1,128,1);
//			digitalWrite(AIN1, HIGH);
//			digitalWrite(AIN2, LOW);
//			analogWrite(PWMA, 128);
			move(4,128,0);
//			digitalWrite(DIN1, LOW);
//			digitalWrite(DIN2, HIGH);
//			analogWrite(PWMD, 128);
			move(2,0,0);
			move(3,0,0);
			break;
		case'e'://原地左转
			
			Serial.println("move");
			move(1,128,1);
			move(2,128,1);
			move(3,128,0);
			move(4,128,0);
			break;
		case'f'://原地左转右，v0__发现没break
			Serial.println("move");
			move(3,128,1);
			move(4,128,1);
			move(1,128,0);
			move(2,128,0);
			break;		
		case'0'://停止
			Serial.println("access 3");
			Serial.println("stop");
			stop();
		i=8;
}
}